//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  Updated Nov-2007 by Trevor Taylor, Software Technology, Australia
//
//  Change Log:
//  Added new service operations for setting I/O pins, playing a tone
//  on the speaker and executing pre-defined behaviors (on the robot).
//
//  Fixed bug where right bumper was not being added to the sensor array.
//
//
//  $File: BasicStamp2.cs $ $Revision: 19 $
//-----------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Net;
using System.Collections.Generic;
using System.ComponentModel;
using System.Security.Permissions;

using Microsoft.Robotics.Services.BasicStamp2;
using W3C.Soap;
using Microsoft.Robotics.Services.BoeBot;

using coord = Microsoft.Robotics.Services.Coordination.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;


namespace Microsoft.Robotics.Services.BasicStamp2
{
    // the boe control code uses a polling loop that blocks a thread
    // The ActivationSettings attribute with Sharing == false makes the runtime
    // dedicate a dispatcher thread pool just for this service.
    [ActivationSettings(ShareDispatcher=false,ExecutionUnitsPerDispatcher=2)]
    [Contract(Contract.Identifier)]
    [DisplayName("Boe-Bot BASIC Stamp 2")]
    [Description("Provides access to the Parallax Boe-Bot using BASIC Stamp 2.")]
    public class BasicStamp2Service : DsspServiceBase
    {
        // TT - Change this value for your setup
        //private const int DefaultSerialPort = 3;
        private const int DefaultSerialPort = 7;

        [InitialStatePartner(Optional=true, ServiceUri="BasicStamp2.config.xml")]
        private BasicStampState _state = null;

        [EmbeddedResource("Microsoft.Robotics.Services.BasicStamp2.BasicStamp2.xslt")]
        string _transform = null;
                
        [ServicePort("/BasicStamp2", AllowMultipleInstances = false)]
        private BasicStamp2Operations _mainPort = new BasicStamp2Operations();

        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        Port<BoeBot.CommandLoop> _controlPort = new Port<CommandLoop>();

        private BoeBotControl _control = null;
        private Dictionary<Guid, Port<UpdateMotorSpeed>> _coordinationList = new Dictionary<Guid, Port<UpdateMotorSpeed>>();

        public BasicStamp2Service(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
			
        }
        protected override void Start()
        {
            InitializeState();

            // Listen on the main port for requests and call the appropriate handler.
            ActivateDsspOperationHandlers();

            // Publish the service to the local Node Directory
            DirectoryInsert();

			// display HTTP service Uri
			LogInfo(LogGroups.Console, "Service uri: ");

            _control = new BoeBotControl(_mainPort);

            // TT - Transfer the setting of Autonomous mode from the State to the
            // BoeBotControl object
            _control.AutonMode = _state.AutonomousMode;

            // TT - This kicks off the Control Loop
            Activate(Arbiter.Receive(false, _controlPort, ControlLoop));
            _controlPort.Post(CommandLoop.START);
        }

        /// <summary>
        /// Validate and Initialize BasicStamp2 state
        /// </summary>
        private void InitializeState()
        {
            bool updated = false;

            if (_state == null)
            {
                updated = true;
                _state = new BasicStampState();
                _state.AutonomousMode = false;
                _state.Connected = false;
                _state.FrameCounter = 0;
            }

            if (_state.Configuration == null)
            {
                updated = true;
                _state.Configuration = new Config();
                _state.Configuration.SerialPort = DefaultSerialPort;
                _state.Configuration.Delay = 0;
            }

            if (_state.Sensors == null)
            {
                updated = true;
                _state.Sensors = new Sensors();
            }

            if (_state.MotorSpeed == null)
            {
                updated = true;
                _state.MotorSpeed = new MotorSpeed();
            }

            if (updated)
                SaveState(_state);

        }

        // TT - This is the main Control Loop which is executed continuously
        // It calls itself repeatedly by posting a message to itself
        void ControlLoop(CommandLoop cmd)
        {
            if (cmd == CommandLoop.STOP)
            {
                _control.Running = false;
                return;
            }
            else if (cmd == CommandLoop.START)
            {
                if (ConnectToBoebot())
                    _control.Start();
                else
                    _state.ConnectAttempts++;
            }

            if (_state.ConnectAttempts > 5)
            {
                LogError(LogGroups.Activation,"Can not connect to the bluetooth serial port. Please make sure the configuration file uses the correct serial port");
                _mainPort.Post(new DsspDefaultDrop());
                return;
            }

            // TT - ExecuteMain does the serial I/O and may therefore block
            _control.ExecuteMain();

            // TT - Set up for callback
            Activate(Arbiter.Receive(false, _controlPort, ControlLoop));
            _controlPort.Post(CommandLoop.START);
        }

        #region Operation Port Handlers
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            Close();
            _state = replace.Body;
            _state.Connected = false;
            _state.Connected = ReConnect();
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success){},
                delegate(Exception ex)
                {
                    LogError(ex);
                    throw ex;
                }
            );

            // TT Jul-2008 - Change for July CTP of 2.0
            SendNotificationToTarget<UpdateMotorSpeed>(subscribe.Body.Subscriber, _subMgrPort, new UpdateMotorSpeed(_state.MotorSpeed));
            SendNotificationToTarget<SensorsChanged>(subscribe.Body.Subscriber, _subMgrPort, new SensorsChanged(_state.Sensors));

            yield break;
        }

        /// <summary>
        /// QueryConfig Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> QueryConfigHandler(QueryConfig query)
        {
            query.ResponsePort.Post(_state.Configuration);
            yield break;
        }

        /// <summary>
        /// QueryInfraRed Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> QueryInfraRedHandler(QueryInfraRed query)
        {
            query.ResponsePort.Post(_state.Sensors);
            yield break;
        }

        /// <summary>
        /// QueryMotorSpeed Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> QueryMotorSpeedHandler(QueryMotorSpeed query)
        {
            query.ResponsePort.Post(_state.MotorSpeed);
            yield break;
        }

        /// <summary>
        /// UpdateConfig Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateConfigHandler(UpdateConfig update)
        {
            if (update.Body.Delay >= 0)
                _state.Configuration.Delay = update.Body.Delay;

            if (update.Body.SerialPort >= 0)
            {
                _state.Configuration.SerialPort = update.Body.SerialPort;
                _state.Connected = ReConnect();
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        public void SetCoordinatedMotors(UpdateMotorSpeed[] motors)
        {
            MotorSpeed motorSpeed = new MotorSpeed();

            // Default -1 which is ignored by the controller.
            motorSpeed.LeftSpeed = null;
            motorSpeed.RightSpeed = null;

            // Combine the motor commands
            foreach (UpdateMotorSpeed ms in motors)
            {
                if (ms.Body.LeftSpeed != null && ms.Body.LeftSpeed >= -1.0 && ms.Body.LeftSpeed <= 1.0)
                        motorSpeed.LeftSpeed = ms.Body.LeftSpeed;

                if (ms.Body.RightSpeed != null && ms.Body.RightSpeed >= -1.0 && ms.Body.RightSpeed <= 1.0)
                        motorSpeed.RightSpeed = ms.Body.RightSpeed;
            }

            // Send a single command to the Boe-Bot
            UpdateMotorSpeed combinedRequest = new UpdateMotorSpeed(motorSpeed);
            _mainPort.Post(combinedRequest);
            Activate(Arbiter.Choice(combinedRequest.ResponsePort,
                delegate(DefaultUpdateResponseType response)
                {
                    // send responses back to the original motors
                    foreach (UpdateMotorSpeed ms in motors)
                        ms.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                },
                delegate(Fault fault)
                {
                    // send failure back to the original motors
                    foreach (UpdateMotorSpeed ms in motors)
                        ms.ResponsePort.Post(fault);

                }));

        }

        /// <summary>
        /// UpdateMotorSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateMotorSpeedHandler(UpdateMotorSpeed update)
        {

            coord.ActuatorCoordination coordination = update.GetHeader<coord.ActuatorCoordination>();
            if (coordination != null)
            {
                if (!_coordinationList.ContainsKey(coordination.RequestId))
                {
                    _coordinationList.Add(coordination.RequestId, new Port<UpdateMotorSpeed>());
                    Activate(
                        Arbiter.MultipleItemReceive<UpdateMotorSpeed>(
                            false,
                            _coordinationList[coordination.RequestId],
                            coordination.Count,
                            SetCoordinatedMotors));
                }
                _coordinationList[coordination.RequestId].Post(update);
                yield break;
            }


            bool changed = ((update.Body.LeftSpeed >= 0 && _state.MotorSpeed.LeftSpeed != update.Body.LeftSpeed)
                || (update.Body.RightSpeed >= 0 && _state.MotorSpeed.RightSpeed != update.Body.RightSpeed));

            if (update.Body.LeftSpeed != null && update.Body.LeftSpeed >= -1.0 && update.Body.LeftSpeed <= 1.0)
                    _state.MotorSpeed.LeftSpeed = update.Body.LeftSpeed;

            if (update.Body.RightSpeed != null && update.Body.RightSpeed >= -1.0 && update.Body.RightSpeed <= 1.0)
                _state.MotorSpeed.RightSpeed = update.Body.RightSpeed;

            // If we are connected, send the speed to the robot
            if (ConnectToBoebot())
                _control.SetSpeed(_state.MotorSpeed.LeftSpeed, _state.MotorSpeed.RightSpeed);

            // Send Notifications to subscribers
            if (changed)
            {
                //_subMgrPort.Post(new submgr.Submit(_state.MotorSpeed, DsspActions.UpdateRequest));
                SendNotification<UpdateMotorSpeed>(_subMgrPort, new UpdateMotorSpeed(_state.MotorSpeed)); 
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// UpdateInfraRed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateSensorsHandler(SensorsChanged update)
        {
            _state.Sensors.IRLeft = update.Body.IRLeft;
            _state.Sensors.IRRight = update.Body.IRRight;
            _state.Sensors.WhiskerLeft = update.Body.WhiskerLeft;
            _state.Sensors.WhiskerRight = update.Body.WhiskerRight;

            // Send Notifications to subscribers
            SendNotification<SensorsChanged>(_subMgrPort, new SensorsChanged(_state.Sensors));

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// UpdateAutonomousMode Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateAutonomousModeHandler(UpdateAutonomousMode update)
        {
            _state.AutonomousMode = update.Body.Mode;
            if (ConnectToBoebot())
                _control.AutonMode = _state.AutonomousMode;

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /////////////////////////////////////////////////////////////
        //++
        // TT Oct-2007 - Add request handlers for I/O pins, Speaker and Behaviors

        /// <summary>
        /// SetPins Handler
        /// </summary>
        /// <param name="pins"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetPinsHandler(SetPins pins)
        {
            if (ConnectToBoebot())
                _control.SetPinsRequest(pins.Body.Op1, pins.Body.Pin1, pins.Body.Op2, pins.Body.Pin2);

            pins.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// PlayTone Handler
        /// </summary>
        /// <param name="tone"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> PlayToneHandler(PlayTone tone)
        {
            if (ConnectToBoebot())
                _control.PlayToneRequest(tone.Body.Frequency, tone.Body.Duration);

            tone.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// ExecuteBehavior Handler
        /// </summary>
        /// <param name="behavior"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ExecuteBehaviorHandler(ExecuteBehavior behavior)
        {
            if (ConnectToBoebot())
            {
                _control.SetSpeed(0, 0);
                _control.BehaviorRequest(behavior.Body.Code, behavior.Body.Duration);
            }

            behavior.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        //--
        /////////////////////////////////////////////////////////////


        [ServiceHandler(ServiceHandlerBehavior.Teardown)]
        public virtual IEnumerator<ITask> DropHandler(DsspDefaultDrop drop)
        {
            _control.Stop();
            base.DefaultDropHandler(drop);
            yield break;
        }

        #endregion


        #region Boebot Connections
        /// <summary>
        /// Connect and update Boe-Bot with current settings
        /// </summary>
        private bool ReConnect()
        {
            Close();
            bool connected = ConnectToBoebot();
            if (connected)
            {
                if (_state.AutonomousMode)
                {
                    _control.AutonMode = true;
                }
                else if (_state.MotorSpeed.LeftSpeed != 0 || _state.MotorSpeed.RightSpeed != 0)
                {
                    _control.SetSpeed(_state.MotorSpeed.LeftSpeed, _state.MotorSpeed.RightSpeed);
                }
            }
            return connected;
        }

        /// <summary>
        /// Close the underlying connection to the Boe-Bot.
        /// <remarks>Modifies _state</remarks>
        /// </summary>
        private void Close()
        {
            if (_state.Connected)
            {
                _control.Close();
                _state.Connected = false;
            }
        }

        /// <summary>
        /// Connect to the underlying Boe-Bot
        /// <remarks>Modifies _state</remarks>
        /// </summary>
        /// <returns></returns>
        private bool ConnectToBoebot()
        {
            if (_state.Connected)
                return true;

            string errorMessage;
            _state.Connected = _control.Connect(_state.Configuration.SerialPort, out errorMessage);
            if (!_state.Connected && !string.IsNullOrEmpty(errorMessage))
            {
                LogError(LogGroups.Activation, errorMessage);
            }

            return _state.Connected;
        }
        #endregion
    }
}
